﻿#ifndef hardware_variable_H
#define hardware_variable_H


#include "hardware_interface/Motor_API/function.h"
#include "math.h"
#include <iostream>
#include <vector>
#include <string>
#include <QDebug>
#include <QList>
#include <thread>
#include "SurgicalRobotCommon.h"
#include <QThread>
#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
//simga.7主手实时数据获取类结构体
struct SRobotHapticDeviceDataV2{
    //是否连接
    bool bRobotHapticDeviceConnect;

    //位置平移
    double dhd_px, dhd_py, dhd_pz;
    Eigen::Vector3d position;

    //姿态旋转
    double dhd_ox, dhd_oy, dhd_oz;
    Eigen::Matrix3d dhd_orientation;

    //位置与姿态构成的4阶矩阵
    Eigen::Matrix4d dhd_PostureMatrix;

    //simga.7主手夹持角度
    double dChipGrap;

};

//NDI实时数据获取类结构体
struct SRobotNDIDeviceDataV2
{
    double DATA[10];//qw,qx,qy,qz,TX,TY,TZ
    QString state;//miss/ok/outofboundry
    bool Isvalid;//这个数据是否有效

    QString singlemessage;//one request single feedback
    QString totalmessage;//one request total feedback
};


enum class RobotFollowMode {
  FollowMode_Joint=0,
  FollowMode_TCP=1,

};

enum class RobotFollowSource {
    FollowSource_virtual=0,
    FollowSource_segma=1,
    FollowSource_ndi=2,
    FollowSource_camera=3,

};

enum class RobotFollowstrategy {
    Followstrategy_5times=0,
    Followstrategy_sinV1=1,
    Followstrategy_sinV2=1,

};

//主从跟随设定参数结构
struct RobotFollowPara {
    RobotFollowMode FollowMode;//跟随模式
    RobotFollowSource FollowSource;//跟随数据源（
    RobotFollowstrategy Followstrategy;//跟随策略（0=5次，1=SIN，2=...）
    int FollowSourceIndex;//跟随硬件下标（部分硬件不止一个接入，如果只有一个，则无所谓）

    //...
};

//机械臂实时数据
struct SRobotRealTimeDataV2 {

    //这里应该只包括机器人的数据
    double Joint_Pos[10];
    double Joint_Vel[10];
    double Joint_Acc[10];
    double Joint_Torque[10];

    double TCP_Pos[10];
    double TCP_Vel[10];
    double TCP_Acc[10];
    double TCP_Force[10];
    Eigen::Matrix4d TCPMatrix;//因为各家机器人的tcp姿态定义不同，每次采集到数据都自己转换到44矩阵

    bool isValid;
    int RobotState;

};
//机械臂实时数据
// struct SRobotRealTimeData{

//     //这里应该只包括机器人的数据
//     double Joint_Pos[10];
//     double Joint_Vel[10];
//     double Joint_Acc[10];
//     double Joint_Torque[10];

//     double TCP_Pos[10];
//     double TCP_Vel[10];
//     double TCP_Acc[10];
//     double TCP_Force[10];
//     Eigen::Matrix4d TCPMatrix;//因为各家机器人的tcp姿态定义不同，每次采集到数据都自己转换到44矩阵

//     bool isValid;
//     int RobotState;

// };

//末端器械实时动态数据结构体
struct sEndToolRealTimeDATAV2
{
    //joint pos
    LREALArray10 JointPos;

    //tcp pos
    //末端所夹持的手术器械位置
    Eigen::Vector3d v3dToolPosture;
    double dToolPx, dToolPy, dToolPz;
};

//这个是存储了硬件相关全局变量的
class hardware_variable :public QThread
{
    //如果没有这一句，则无法使用signal机制传递消息(此外类必须继承自确实是Q_OBJECT的对象)
    Q_OBJECT

public:
    hardware_variable();
    ~hardware_variable();

    //这样写就所有类的实例都是指向唯一对象了
    static hardware_variable *instance(void);

    //segma力控设备数据
    //!!!!传感器类的实例化一旦建立，可以从实例访问，也可以从这种数组访问
    SRobotHapticDeviceDataV2 sHapticRunData[2];//左右手2个

    //NDI实时数据（被当做3D跟踪来用）
    SRobotNDIDeviceDataV2 sNDIRunData;

    //视觉信息
    bool Ifcameraopen =false;
    bool IfframeOK;
    cv::Mat frame;
    cv::VideoCapture capture;
    QImage  image;//这个是检测到的图像
    double rate; //FPS
    vector<cv::Point> Points;
    cv::Point Point;

    //CameraProc data

    double Proc_r=40;
    double Proc_R=200;
    double Proc_k=0.5;



    //机械臂实时数据(SurgicalRobotManager.cpp是可以访问到每个机器人实例的获取数据包方法，所以暂时不需要引入)
    //sRobotRunData[0].sRobotDy.m4dRobotEnd;//获取初始机械臂末端位姿矩阵
    //SRobotRealTimeData sRobotRunData[MaxRobotNum];
    //SRobotRealTimeDataV2 sRobotRunData[MaxRobotNum];
    std::array<double, 7> robot_q_d;
    Eigen::Matrix4d robot_m4d,Stiffness_frame_pose_m4d;

    //
    double dettime=0.001;
    string folderPath;


    // Handle force sensor
    Eigen::Matrix<double,8,8> handle_output;
    double H_output[8][8];
    double H_output_sum = 0.0;

    //末端器械实时数据
    Eigen::Matrix4d m4dToolendCurrent;

    int countvalue;
    int testvalueX;
    int testvalueY;
    int testvalueZ;

    bool Ifm4dNextRecorded;
    Eigen::Matrix4d m4dNextRecord;

    Eigen::Matrix4d m4dNext;
    Eigen::Matrix4d m4dToolEndNext;

    Eigen::Vector3d v3dRCMTemp;
    Eigen::Matrix3d m3dOri;
    Eigen::Vector3d v3dTransposition;

    ///AAAAAAAAAAAAA2 m4dNext=
    //simgle cycle data
    bool m4dNextRecordListIfRecordStart=false;
    QList<Eigen::Matrix4d> m4dNextRecordList;


    double segma_startx=0;
    double segma_starty=0;
    double segma_startz=0;

    //force sensor data
    double force_sensor[6];
    double handle_predict[6];
    double handle_predict_new[6];
    double force_fx=0;
    double force_fy=0;
    double force_fz=0;
    double force_tx=0;
    double force_ty=0;
    double force_tz=0;
    bool handle_open;
    int t_begin_force_handle_record=0;
     //Start and Stop
    bool ifTrackMove=false;
    bool ifZoomIn=false;
    bool ifZoomOut=false;
    bool ifpause=false;
    bool ifSaveVedio=false;
    bool ifForceSensor=false;

    QMutex mutex;





};

#endif // hardware_variable_H
